/*
 * Simple class to integrate angle moves
 */
#include "sensors_read.h"
#include "gyro_manager.h"

namespace sensors_test {

    void GyroManager::reset() {
        // Reset angle and empty the AnglesQueue
        angle_ = 0;
        while(angles_queue_->Deq(NULL));
    }

    int GyroManager::get_angle() {
        // Integrate all angle moves
        int delta_angle;
        while(angles_queue_->Deq(&delta_angle)) {
            angle_ += delta_angle;
        }
        return angle_/1000;
    }
                

}
